/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;

/**
 * Same as robot drive, but overrides the tankDrive method to use a cubic
 * function instead of a quadratic.
 * @author Administrator
 */
public class Team997Drive extends RobotDrive {

   /**
     * Constructor for Team997RobotDrive with a default sensitivity
     * @param frontLeftMotor Front left motor port
     * @param rearLeftMotor Rear left motor port
     * @param frontRightMotor Front right motor port
     * @param rearRightMotor Rear right motor port
     */
    public Team997Drive(final int frontLeftMotor, final int rearLeftMotor,
            final int frontRightMotor, final int rearRightMotor) {
        super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor, kDefaultSensitivity);
    }

   /**
     * Constructor for Team997RobotDrive with a default sensitivity
     * @param frontLeftMotor Front left motor
     * @param rearLeftMotor Rear left motor
     * @param frontRightMotor Front right motor
     * @param rearRightMotor Rear right motor
     */
    public Team997Drive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
            SpeedController frontRightMotor, SpeedController rearRightMotor) {
        super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
    }
    /**
     * Provide tank steering using the stored robot configuration.
     * This function lets you directly provide joystick values from any source.
     * Overrides the original version of this function to cube the values, which
     * makes driving easier.
     * @param leftValue The value of the left stick.
     * @param rightValue The value of the right stick.
     */
    public void tankDrive(double leftValue, double rightValue) {
        // square the inputs (while preserving the sign) to increase fine control while permitting full power
        leftValue = limit(leftValue);
        rightValue = limit(rightValue);

        // Cube the value to give more sensitive control.
        leftValue = (leftValue * leftValue * leftValue);

        rightValue = (rightValue * rightValue * rightValue);

        setLeftRightMotorSpeeds(leftValue, rightValue);
    }

    /**
     * Limit motor values to the -1.0 to +1.0 range.
     * @param num
     * @return
     */
    private double limit(double num) {
        if (num > 1.0) {
            return 1.0;
        }
        if (num < -1.0) {
            return -1.0;
        }
        return num;
    }
}
